package com.zhdl.modules.slagRemoval;

import com.zhdl.hardware.io.smartLink.repository.impl.IOCmdHandleDaoImpl;
import com.zhdl.hardware.io.smartLink.service.IOCmdSendService;
import com.zhdl.hardware.io_socket.service.IOWebsocketService;
import com.zhdl.hardware.motor.huichuan.repository.impl.HcPortCmdHandle;
import com.zhdl.hardware.motor.huichuan.service.HCCmdSendService;
import com.zhdl.modules.autopick.entity.Coordinate;
import com.zhdl.network.service.conf.PortConf;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import javax.annotation.Resource;
import java.util.List;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;

@Slf4j
@Component
public class SlagRemovalProcess_Old {

    /**
     * 除渣流程-当前模式， 流程进行
     */
    public static boolean mode = false, isRunning = false;
    /**
     * 电机的对应坐标
     */
    public static Coordinate coordinate = new Coordinate();
    /**
     * 除渣流程进度-南
     */
    public int processSouth = -1, processNorth = -1;
    /**
     * IO信号01，IO信号02
     */
    public List<Byte> IO01, IO02;
    public int processThree, xid, zid;
    /**
     * Sensor 对应电机坐标的状态
     */
    Map<String, String> sensorStatus = new ConcurrentHashMap<>();
    @Resource
    private HCCmdSendService hcCmdSendService;
    @Resource
    private IOCmdSendService ioCmdSendService;
    @Resource
    private IOWebsocketService ioWebsocketService;
    @Resource
    private IOCmdHandleDaoImpl ioCmdHandleDao;
    @Resource
    private HcPortCmdHandle hcPortCmdHandle;
    @Resource
    private PortConf portConf;

    /**
     * 流程开始初始化
     */
    public void init() {
        /* 获取对应的属性名称 */
        hcPortCmdHandle.addPort("7");
        hcPortCmdHandle.addPort("8");
        ioCmdHandleDao.init(portConf.getTcpPortIo());
        /* 读取IO - 判断XZ电机是否在原点 && 传感器是否报警 */
        log.info("判断备战席-是否报警");
    }

    /**
     * 除渣流程_开始入口
     */
    public void processAutomaticMode() {

        /*
            判断是否是在调试状态 如果在就退出流程
         */
        if (mode || isRunning) {
            return;
        }
        isRunning = true;

        log.info("------------------流程开始运动------------------");

        processNorth = 0;
        nextIntNorth();
    }

    /**
     * 除渣流程_开始入口
     */
    public void processManualMode(int xid, int zid) {

        /*
            判断是否是在调试状态 如果在就退出流程
         */
        if (!mode || isRunning) {
            return;
        }
        isRunning = true;

        log.info("------------------流程开始运动------------------");

        this.xid = xid;
        this.zid = zid;

        processThree = 0;
        nextIntMoveToDesk();

    }

    /**
     * 手动模式下-提前笼子后续步骤
     */
    public void nextIntMoveToDesk() {
        processThree++;
        switch (processThree) {

            case 1 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("箱内笼子: Z移动到提笼移动高度");
                });
            }
            case 2 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntMoveToDesk();
                    log.info("箱内笼子: X移动到绕梁高度位置");
                });
            }
            case 3 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("箱内笼子: Z上升过箱高度");
                });
            }
            case 4 -> {
                if (zid == 1) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX3(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("箱内笼子: X移动到桌上放笼高度 id");
                    });
                } else if (zid == 2) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX4(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("箱内笼子: X移动到桌上放笼高度 id");
                    });
                } else if (zid == 3) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX5(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("箱内笼子: X移动到桌上放笼高度 id");
                    });
                }
            }
            case 5 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("箱内笼子: Z在桌子上放下笼子");
                });
            }
            case 6 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntMoveToDesk();
                            log.info("箱内笼子: P松开笼子");
                        });
                    });
                });

            }
            case 7 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ4(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("箱内笼子: Z上升至桌上可移动高度");
                });
            }
            case 8 -> {
                if (zid == 1) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX5(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("桌上笼子: X移动至 id - 1 的位置，当前位置需要判断");
                    });
                } else if (zid == 2) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX3(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("桌上笼子: X移动至 id - 1 的位置，当前位置需要判断");
                    });
                } else if (zid == 3) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX4(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("桌上笼子: X移动至 id - 1 的位置，当前位置需要判断");
                    });
                }
            }
            case 9 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: Z下降至可抓取笼子高度");
                });
            }
            case 10 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPC(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntMoveToDesk();
                            log.info("桌上笼子: P抓紧笼子");
                        });
                    });
                });

            }
            case 11 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: Z上升至可越过箱子高度");
                });
            }
            case 12 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: X右移动至过箱子位置");
                });
            }
            case 13 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: Z下降至绕梁高度");
                });
            }
            case 14 -> {
                if (xid == 1) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX0(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("根据id判断 应该去哪里放笼子，然后调用 ProcessOne or ProcessTwo");
                    });
                } else if (xid == 2) {
                    hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX1(), 1200, s -> {
                        nextIntMoveToDesk();
                        log.info("根据id判断 应该去哪里放笼子，然后调用 ProcessOne or ProcessTwo");
                    });
                }
            }

            case 15 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ0(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: Z下降至放笼子高度");
                });
            }
            case 16 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntMoveToDesk();
                            log.info("桌上笼子: P松开笼子");
                        });
                    });
                });

            }
            case 17 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: Z上升至原点位置");
                });
            }
            case 18 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX0(), 1200, s -> {
                    nextIntMoveToDesk();
                    log.info("桌上笼子: X移动至原点位置");
                });
            }
            case 19 -> {
                isRunning = false;
            }
        }
    }

    /**
     * 电机流程-北
     */
    public void nextIntNorth() {
        processNorth++;
        switch (processNorth) {
            case 1 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ0(), 2000, 0, 2000, s -> {
                    log.info("电机第一次下降完成");
                    nextIntNorth();
                });
            }
            case 2 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPC(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机抓取完成,状态码为:" + s11);
                        });
                    });
                });

            }
            case 3 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    log.info("电机Z移动至过底部笼高度");
                    nextIntNorth();
                });
            }
            case 4 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X移动至绕过梁位置,状态码为:" + s);
                });
            }
            case 5 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    log.info("电机Z移动至提篮越过水池高度");
                    nextIntNorth();
                });
            }
            case 6 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX3(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X移动桌子第一个位置:" + s);
                });
            }
            case 7 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y移动至桌子放下笼子位置");
                });
            }
            case 8 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机松开完成,状态码为:" + s1);
                        });
                    });
                });
            }
            case 9 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ4(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机第三次上升完成,状态码为:");
                });
            }
            case 10 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX4(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机x移动至2笼子位置" + s);
                });
            }
            case 11 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机y抓起笼子位置");
                });
            }
            case 12 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPC(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机第二次抓取完成,状态码为:" + s1);
                        });
                    });
                });
            }
            case 13 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y到越过水池高度");
                });
            }
            case 14 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机x运动至绕梁位置:" + s);
                });
            }
            case 15 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y下降至能绕梁高度");
                });
            }
            case 16 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX0(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X回到原点准备放笼子" + s);
                });
            }
            case 17 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ0(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y移动池子放笼高度");
                });
            }
            case 18 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机第二次松开完成,状态码为:" + s1);
                        });
                    });
                });
            }
            case 19 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y下降至能绕梁高度1");
                });
            }
            case 20 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX1(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X水池第二个笼子位置" + s);
                });
            }
            case 21 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ01(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y移动池子放笼高度2");
                });
            }
            case 22 -> {
                ioCmdSendService.ioWriteCoil05(2, false, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPC(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机左边抓取完成,状态码为:" + s1);
                        });
                    });
                });
            }
            case 23 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y上升至能绕梁高度1");
                });
            }

            case 24 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X移动至绕过梁位置,状态码为:1" + s);
                });
            }
            case 25 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    log.info("电机Z移动至提篮越过水池高度1");
                    nextIntNorth();
                });
            }
            case 26 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX4(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X移动桌子第一个位置:1" + s);
                });
            }
            case 27 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y移动至桌子放下笼子位置1");
                });
            }
            case 28 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机松开完成,状态码为:1" + s1);
                        });
                    });
                });
            }
            case 29 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ4(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机第三次上升完成,状态码为:1");
                });
            }
            case 30 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX5(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机x移动至2笼子位置1" + s);
                });
            }
            case 31 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ3(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机y抓起笼子位置1");

                });
            }
            case 32 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPC(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机第二次抓取完成,状态码为:1" + s1);
                        });
                    });
                });
            }
            case 33 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ2(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y到越过水池高度1");
                });
            }
            case 34 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX2(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机x运动至绕梁位置:1" + s);
                });
            }
            case 35 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y下降至能绕梁高度11");
                });
            }

            case 36 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX1(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X回到X1准备放笼子" + s);
                });
            }
            case 37 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ01(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y移动池子放笼高度3");
                });
            }
            case 38 -> {
                ioCmdSendService.ioWriteCoil05(2, true, s1 -> {
                    hcCmdSendService.motorMoveControl("7", "03", coordinate.getLeftPO(), 1000, 0, 2000, s -> {
//                        hcCmdSendService.motorStatusStop("7","03");
                        ioCmdSendService.ioWriteCoil05(2, false, s11 -> {
                            nextIntNorth();
                            log.info("电机第二次松开完成2,状态码为:" + s1);
                        });
                    });
                });
            }
            case 39 -> {
                hcCmdSendService.motorMoveControl("7", "02", coordinate.getLeftZ1(), 2000, 0, 2000, s -> {
                    nextIntNorth();
                    log.info("电机Y上升至能绕梁高度2");
                });
            }
            case 40 -> {
                hcCmdSendService.motorMoveDistance("7", "01", coordinate.getLeftX0(), 1200, s -> {
                    nextIntNorth();
                    log.info("电机X原点位置 完成流程" + s);
                });
            }
            case 41 -> {
                isRunning = false;
            }

        }
    }

    public void finish() {

    }

//    public void nextIntOne(){
//        processOne++;
//        switch (processOne){
//            case 1 -> {
//                nextIntOne();
//                log.info("箱内笼子右: 下降至笼子高度Z");
//            }
//            case 2 -> {
//                nextIntOne();
//                log.info("箱内笼子右: 抓起笼子P");
//            }
//            case 3 -> {
//                nextIntOne();
//                log.info("箱内笼子右: 提升至可查看高度");
//            }
//
//            case 11 -> {
//                nextIntOne();
//                log.info("箱内笼子右: 下降至笼子高度Z");
//            }
//            case 12 -> {
//                nextIntOne();
//                log.info("箱内笼子右: 松开笼子P");
//            }
//            case 13 ->{
//                nextIntOne();
//                log.info("箱内笼子右: 至原点位置P");
//            }
//        }
//    }
//
//    public void nextIntTwo(){
//        processTwo++;
//        switch (processTwo){
//            case 1 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 上升至可移动高度Z");
//            }
//            case 2 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 向左移动到二号位置X");
//            }
//            case 3 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 下降至笼子高度Z");
//            }
//            case 4 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 抓起笼子P");
//            }
//            case 5 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 提升至可查看高度");
//            }
//
//            case 11 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 下降至笼子高度Z");
//            }
//            case 12 -> {
//                nextIntTwo();
//                log.info("箱内笼子左: 松开笼子P");
//            }
//            case 13 ->{
//                nextIntTwo();
//                log.info("箱内笼子左: 上升至可移动高度Z");
//            }
//            case 14 ->{
//                nextIntTwo();
//                log.info("箱内笼子左: 平移到原点位置X");
//            }
//            case 15 ->{
//                nextIntTwo();
//                log.info("箱内笼子左: 下降至原点位置P");
//            }
//        }
//    }

    /**
     * 电机流程-南
     */
    public void nextIntSouth() {
        processNorth++;
        switch (processNorth) {
            case 1 -> {
                hcCmdSendService.motorMoveControl("2", "04", coordinate.getCoordinate_z(), 400, 0, 2000, s -> {
                    hcCmdSendService.motorStatusStop("2", "04");
                    ioCmdSendService.ioWriteCoil05(1, false, s1 -> {
                        nextIntNorth();
                        log.info("电机下降完成,状态码为:" + s1);
                    });
                });
            }
            case 2 -> {
                hcCmdSendService.motorMoveControl("2", "06", coordinate.getCoordinate_p(), 200, 0, 2000, s -> {
                    hcCmdSendService.motorStatusStop("2", "06");
                    ioCmdSendService.ioWriteCoil05(2, false, s1 -> {
                        nextIntNorth();
                        log.info("电机抓取完成,状态码为:" + s1);
                    });
                });
            }
            case 3 -> {
                hcCmdSendService.motorMoveControl("2", "04", coordinate.getCoordinate_z(), 400, 0, 2000, s -> {
                    hcCmdSendService.motorStatusStop("2", "04");
                    ioCmdSendService.ioWriteCoil05(1, false, s1 -> {
                        nextIntNorth();
                        log.info("电机上升完成,状态码为:" + s1);
                    });
                });
            }
            case 4 -> {
                hcCmdSendService.motorMoveDistance("2", "03", coordinate.getCoordinate_x(), 400, s -> {
                    nextIntNorth();
                    log.info("电机运动完成,状态码为:" + s);
                });
            }
        }
    }
}
